﻿Imports System
Imports System.Net
Imports System.Net.Sockets
Imports System.Collections.Concurrent
Imports System.Collections.Generic
Imports System.Threading
Imports System.Threading.Tasks
Imports Alchemy
Imports Alchemy.Classes
Imports Newtonsoft.Json
Imports SlimDX
Imports SlimDX.XInput

Public Class Client
    '********** Client Variables **********
    Dim wsClient                                             ' Stores an instance of the Server
    Delegate Sub UpdateConsoleCallback(ByVal [s] As String)  ' Delegate to update UI

    Dim port = 5001 ' WebSocket Server Port
    Dim connected = False ' Is the WebSocket Server Connected?
    Dim actuatorState = "stopped" ' Currently
    Dim bActuatorState = "stopped"
    Dim brushState = "stopped"
    Dim driveState = "stopped"

    Private Controllers As Controller() = New Controller(UserIndex.One) {}
    Public Sub New()
        InitializeComponent()
        Controllers(0) = New Controller(UserIndex.One)
    End Sub
    ' Data Structure to Store Message from Client
    Public Class Message
        Public msg As String

    End Class

    '**********************************************
    '*************** Click Handlers ***************
    '**********************************************

    ' BtnBrush 0% Power Click Handler
    Private Sub BtnBrush_power0_Click(sender As Object, e As EventArgs) Handles BtnBrush_power0.Click
        BrushPower(0)
    End Sub

    ' BtnBrush 25% Power Click Handler
    Private Sub BtnBrush_power25_Click(sender As Object, e As EventArgs) Handles BtnBrush_power25.Click
        BrushPower(25)
    End Sub

    ' BtnBrush 50% Power Click Handler
    Private Sub BtnBrush_power50_Click(sender As Object, e As EventArgs) Handles BtnBrush_power50.Click
        BrushPower(50)
    End Sub

    ' BtnBrush 75% Power Click Handler
    Private Sub BtnBrush_power75_Click(sender As Object, e As EventArgs) Handles BtnBrush_power75.Click
        BrushPower(75)
    End Sub

    ' BtnBrush 100% Power Click Handler
    Private Sub BtnBrush_power100_Click(sender As Object, e As EventArgs) Handles BtnBrush_power100.Click
        BrushPower(100)
    End Sub

    ' BtnForward Click Handler
    Private Sub BtnForward_Drivetrain_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles BtnForward_Drivetrain.Click
        Forward()
    End Sub

    ' BtnBackward Click Handler
    Private Sub BtnBackward_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnBackward_Drivetrain.Click
        Backward()
    End Sub

    ' BtnLeft Click Handler
    Private Sub BtnLeft_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnLeft_Drivetrain.Click
        GoLeft()
    End Sub

    ' BtnRight Click Handler
    Private Sub BtnRight_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnRight_Drivetrain.Click
        GoRight()
    End Sub

    ' BtnStop Click Handler
    Private Sub BtnStop_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnStop_Drivetrain.Click
        StopMoving()
    End Sub

    ' BtnUp Click Handler
    Private Sub BtnUp_Actuator_Click(sender As Object, e As EventArgs) Handles BtnUp_Actuator.Click
        ActuatorUp()
    End Sub

    ' BtnDown Click Handler
    Private Sub BtnDown_Actuator_Click(sender As Object, e As EventArgs) Handles BtnDown_Actuator.Click
        ActuatorDown()
    End Sub

    ' BtnStop Click Handler
    Private Sub BtnStop_Actuator_Click(sender As Object, e As EventArgs) Handles BtnStop_Actuator.Click
        ActuatorStop()
    End Sub

    ' BtnTest Click Handler
    Private Sub BtnTest_13_Click(sender As Object, e As EventArgs) Handles BtnTest_13.Click
        Test()
    End Sub

    '********************************************
    '*************** Control Subs ***************
    '********************************************

    ' Set Brush Power
    Public Sub BrushPower(ByVal power As Integer)

        ' Writes correct speed on transmit pin
        Select Case (power)
            Case 0 To 50
                If Not brushState = "stopped" Then
                    display("Brush Stopped")
                    wsClient.Send("15")
                    brushState = "stopped"
                End If
            Case 51 To 101
                If Not brushState = "25" Then
                    display("Brush at 25%")
                    wsClient.Send("14")
                    brushState = "25"
                End If
            Case 102 To 152
                If Not brushState = "50" Then
                    display("Brush at 50%")
                    wsClient.Send("13")
                    brushState = "50"
                End If
            Case 153 To 203
                If Not brushState = "75" Then
                    display("Brush at 75%")
                    wsClient.Send("12")
                    brushState = "75"
                End If
            Case 204 To 255
                If Not brushState = "100" Then
                    display("Brush at Full")
                    wsClient.Send("11")
                    brushState = "100"
                End If
        End Select
    End Sub

    ' Go Forwards
    Public Sub Forward()
        If Not driveState = "forward" Then
            display("Going Forward")
            wsClient.Send("01")
            driveState = "forward"
        End If
    End Sub

    ' Go Backwards
    Public Sub Backward()
        If Not driveState = "backward" Then
            display("Going Backward")
            wsClient.Send("02")
            driveState = "backward"
        End If
    End Sub

    ' Go Left
    Public Sub GoLeft()
        If Not driveState = "left" Then
            display("Going Left")
            wsClient.Send("03")
            driveState = "left"
        End If
    End Sub

    ' Go Right
    Public Sub GoRight()
        If Not driveState = "right" Then
            display("Going Right")
            wsClient.Send("04")
            driveState = "right"
        End If
    End Sub

    ' Stop
    Public Sub StopMoving()
        If Not driveState = "stopped" Then
            display("Stopping")
            wsClient.Send("00")
            driveState = "stopped"
        End If
    End Sub

    ' Actuator Up
    Public Sub ActuatorUp()
        If Not actuatorState = "up" Then
            display("Actuator Going Up")
            wsClient.Send("05")
            actuatorState = "up"
        End If
    End Sub

    ' Actuator Down
    Public Sub ActuatorDown()
        If Not actuatorState = "down" Then
            display("Actuator Going Down")
            wsClient.Send("06")
            actuatorState = "down"
        End If
    End Sub

    ' Actuator Stop
    Public Sub ActuatorStop()
        If Not actuatorState = "stopped" Then
            display("Actuator Stopping")
            wsClient.Send("07")
            actuatorState = "stopped"
        End If
    End Sub

    ' Back Actuator Up
    Public Sub BActuatorUp()
        If Not bActuatorState = "up" Then
            display("Back Actuator Going Up")
            wsClient.Send("08")
            bActuatorState = "up"
        End If
    End Sub

    ' Back Actuator Down
    Public Sub BActuatorDown()
        If Not bActuatorState = "down" Then
            display("Back Actuator Going Down")
            wsClient.Send("09")
            bActuatorState = "down"
        End If
    End Sub

    ' Back Actuator Stop
    Public Sub BActuatorStop()
        If Not bActuatorState = "stopped" Then
            display("Back Actuator Stopping")
            wsClient.Send("10")
            bActuatorState = "stopped"
        End If
    End Sub

    ' Test Pin 13 LED
    Public Sub Test()
        display("Testing LED on Arduino Pin 13")
        wsClient.Send("Test")
    End Sub

    '********************************************
    '****************** Client ******************
    '********************************************

    'Update Console Display - DO NOT CALL
    Sub updateConsole(ByVal [s] As String)
        Me.consoleBox.AppendText(DateTime.Now.TimeOfDay.ToString + " " + s + vbCrLf)
    End Sub

    'Decide Method and Update Console Display - Thread Safe
    Private Sub display(ByVal [s] As String)
        If Me.consoleBox.InvokeRequired Then
            Dim d As New UpdateConsoleCallback(AddressOf updateConsole)
            Me.Invoke(d, New Object() {[s]})
        Else
            updateConsole([s])
        End If
    End Sub

    'Start Server Button
    Private Sub btnStartServer_Click(sender As Object, e As EventArgs) Handles btnStartServer.Click
        btnEndServer.Enabled = True
        btnStartServer.Enabled = False
        start()
    End Sub

    'Stop Server Button
    Private Sub btnEndServer_Click(sender As Object, e As EventArgs) Handles btnEndServer.Click
        btnEndServer.Enabled = False
        btnStartServer.Enabled = True
        quit()
    End Sub

    'Server
    Private Sub start()
        consoleBox.AppendText("Middle Tennessee State University" + vbCrLf)
        consoleBox.AppendText("RaiderRobotics 2013 - NASA Lunabotics Competition" + vbCrLf)
        consoleBox.AppendText(vbCrLf)
        consoleBox.AppendText("WebSocket Client:" + vbCrLf)
        consoleBox.AppendText("-----------------------------------------------------------" + vbCrLf)

        'Start WebSocket Server
        display("Starting WebSocket Client")

        'Creates server and binds events
        wsClient = New Alchemy.WebSocketClient("ws://" + txtBoxIpAddress.Text + ":" + port.ToString + "/") With { _
            .OnReceive = AddressOf OnReceive, _
            .OnSend = AddressOf OnSend, _
            .OnConnected = AddressOf OnConnect, _
            .OnDisconnect = AddressOf OnDisconnect _
        }

        Try
            wsClient.Connect()
        Catch ex As Exception
            display("Error connecting.  Exception: " + ex.ToString)
            btnEndServer.Enabled = False
            btnStartServer.Enabled = True
        End Try
    End Sub

    'Handles a Client Connecting
    Public Sub OnConnect(context As UserContext)
        display("Connected.")
        connected = True
    End Sub

    'Handles a Client Disconnecting
    Public Sub OnDisconnect(context As UserContext)
        display("Disconnected.")
        connected = False
    End Sub

    'Handles Message Reciept
    'TODO - Rewrite this to handle JSON messages from Client
    Public Sub OnReceive(context As UserContext)
        Try
            display("Recieved message: " + context.Data.ToString + vbCrLf)
        Catch e As Exception
            display("Bad Message Recieved")
        End Try
    End Sub

    'Notification that a Message was Sent
    Public Sub OnSend(context As UserContext)
        display("Sent message")
    End Sub

    'Quit 
    Public Sub quit()
        wsClient.Disconnect()
    End Sub

    Private Sub Client_Load(sender As Object, e As EventArgs) Handles MyBase.Load
        Timer1.Interval() = 50 'Sets the interval for the Timer to be in ms
        Timer1.Start() ' This starts the timer 
    End Sub

    Private Sub Timer1_Tick(sender As Object, e As EventArgs) Handles Timer1.Tick
        ' Xbox 360 Controller Code 
        'Safetey Feature - If the controller is not connected or becomes disconnected while in use all motors are turned off

        ' Dim prevstate ' Needed to be added for latency 
        If Connected Then
            If Controllers(0).IsConnected Then
                Dim currentstate = Controllers(0).GetState.Gamepad.Buttons

                ' Manage the Actuator
                If (currentstate And GamepadButtonFlags.LeftShoulder) <> 0 Then
                    ActuatorDown()
                ElseIf (currentstate And GamepadButtonFlags.RightShoulder) <> 0 Then
                    ActuatorUp()
                Else
                    ActuatorStop()
                End If

                ' Manage the Rear Actuator
                If (currentstate And GamepadButtonFlags.A) <> 0 Then
                    BActuatorDown()
                ElseIf (currentstate And GamepadButtonFlags.X) <> 0 Then
                    BActuatorUp()
                Else
                    BActuatorStop()
                End If


                ' Manage the Drive
                If (currentstate And GamepadButtonFlags.DPadUp) <> 0 Then
                    Forward()
                ElseIf (currentstate And GamepadButtonFlags.DPadDown) <> 0 Then
                    Backward()
                ElseIf (currentstate And GamepadButtonFlags.DPadLeft) <> 0 Then
                    GoLeft()
                ElseIf (currentstate And GamepadButtonFlags.DPadRight) <> 0 Then
                    GoRight()
                Else
                    StopMoving()
                End If

                ' Manage the Brush
                BrushPower((Controllers(0).GetState.Gamepad.RightTrigger))
            End If
            ' prevstate = currentstate
        End If
    End Sub
End Class
